/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <global_planner/planner_core.h>
#include <pluginlib/class_list_macros.hpp>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>

#include <global_planner/dijkstra.h>
#include <global_planner/astar.h>
#include <global_planner/grid_path.h>
#include <global_planner/gradient_path.h>
#include <global_planner/quadratic_calculator.h>


/**
 *  * ROS中的全局规划期global_planner功能包，前面已经介绍了nav_core、move_base、costmap_2d功能包。全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，
ROS中常见的全局规划器功能包有navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt!(常用于机械臂)等.
plan_node文件是全局规划的入口；planner_core是global_planner的核心，进行初始化，调用A*或者Dijkstra进行全局规划；quadratic_calculator（二次逼近方式，常用）
和potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；搜索到路径后使用回溯grid_path（栅格路径）、gradient_path（梯度路径）获得最终路径；之后orientation_filter进行方向滤波。
栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
梯度路径：从周围八个栅格中找到下降梯度最大的点
 https://blog.csdn.net/weixin_44126988/article/details/126101315
 */

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

namespace global_planner {

 //   这段代码实现了一个名为 GlobalPlanner 的类的成员函数 outlineMap()，其作用是将给定的 costarr 数组的边界设置为指定的 value 值。该函数的详细解释如下
/**
 * 具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
 * 然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。

在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
 于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
 * @param costarr
 * @param nx
 * @param ny
 * @param value
 */
void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
    // 首先，将数组第一行全部设置为指定的值
    unsigned char* pc = costarr;
    for (int i = 0; i < nx; i++)
        *pc++ = value;
    // 接着，将数组最后一行全部设置为指定的值
    pc = costarr + (ny - 1) * nx;
    for (int i = 0; i < nx; i++)
        *pc++ = value;
    //  // 然后，将数组第一列全部设置为指定的值
    pc = costarr;
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;

    // // 最后，将数组最后一列全部设置为指定的值
    pc = costarr + nx - 1;
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;

         /**
        pc = costarr + (ny - 1) * nx; 解释一下
        这行代码的作用是让指针 pc 指向数组 costarr 的最后一行的第一个元素。具体而言，由于 costarr 是一个一维数组，按照行优先的顺序存储，因此最后一行的第一个元素的下标应该是 (ny - 1) * nx。
        这里用指针的算术运算 costarr + (ny - 1) * nx 来获得该元素的地址，并将其赋值给指针 pc。这样，接下来就可以通过对 pc 进行递增操作，遍历数组 costarr 的最后一行了。
        */
}

GlobalPlanner::GlobalPlanner() :
        costmap_(NULL), initialized_(false), allow_unknown_(true),
        p_calc_(NULL), planner_(NULL), path_maker_(NULL), orientation_filter_(NULL),
        potential_array_(NULL) {
}

GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
        GlobalPlanner() {
    //initialize the planner
    initialize(name, costmap, frame_id);
}

GlobalPlanner::~GlobalPlanner() {
    if (p_calc_)
        delete p_calc_;
    if (planner_)
        delete planner_;
    if (path_maker_)
        delete path_maker_;
    if (dsrv_)
        delete dsrv_;
}

/**
 * ROS中的全局规划期global_planner功能包，前面已经介绍了nav_core、move_base、costmap_2d功能包。全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，
ROS中常见的全局规划器功能包有navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt!(常用于机械臂)等.
plan_node文件是全局规划的入口；planner_core是global_planner的核心，进行初始化，调用A*或者Dijkstra进行全局规划；quadratic_calculator（二次逼近方式，常用）
和potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；搜索到路径后使用回溯grid_path（栅格路径）、gradient_path（梯度路径）获得最终路径；之后orientation_filter进行方向滤波。
栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
梯度路径：从周围八个栅格中找到下降梯度最大的点
 * @param name
 * @param costmap
 * @param frame_id
 */

///home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_base/src/move_base.cpp  全局路经规划初始化 172
/// planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);//全局路经规划初始化 172
 // /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_base/src/move_base.cpp 166
 //   planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图 166
 //   planner_costmap_ros_->pause();//暂停全局代价地图的更新，但是不会关闭传感器话题的订阅
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
    initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}


/**
 * 进行初始化：如果没有初始化，就执行初始化操作，获取costmap的尺寸大小，初始化参数：old_navfn_behavior_、use_quadratic（是否使用二次逼近：是：调用QuadraticCalculator-推荐，否：调用PotentialCalculator）、use_dijkstra（true:使用dijkstra,false:A*）、use_grid_path（是：栅格路径；否：梯度路径–一般平滑性较好）
————————————————
版权声明：本文为CSDN博主「如阳光如你」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_44126988/article/details/126101315
 * @param name
 * @param costmap
 * @param frame_id
 */
//140行
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
    if (!initialized_) {
        ros::NodeHandle private_nh("~/" + name);
        costmap_ = costmap;
        frame_id_ = frame_id;
         //地图的大小，即像素数。
        unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
//参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
    //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

        private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
        if(!old_navfn_behavior_)
            convert_offset_ = 0.5;//false
        else
            convert_offset_ = 0.0;//true

        bool use_quadratic;
        private_nh.param("use_quadratic", use_quadratic, true);//是否二次逼近获取路径
        if (use_quadratic) //use_quadratic（是否使用二次逼近：是：调用QuadraticCalculator-推荐，否：调用PotentialCalculator）
            p_calc_ = new QuadraticCalculator(cx, cy);//   // cx, cy 地图的大小，即像素数。  实现了 PotentialCalculator接口
        else
            p_calc_ = new PotentialCalculator(cx, cy);

        bool use_dijkstra;// 用a* 还是 dijkstra 做全局规划
        private_nh.param("use_dijkstra", use_dijkstra, true);
        if (use_dijkstra)
        {
            //// cx, cy 地图的大小，即像素数。
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);//true
            if(!old_navfn_behavior_)
                de->setPreciseStart(true);
            planner_ = de;
        }
        else
            planner_ = new AStarExpansion(p_calc_, cx, cy);//false  // cx, cy 地图的大小，即像素数。

        bool use_grid_path;//是否使用栅格路径  如果如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.
        private_nh.param("use_grid_path", use_grid_path, false);
        if (use_grid_path)
            path_maker_ = new GridPath(p_calc_);//new 出path_maker_实例，从可行点中提取路径
        else
            path_maker_ = new GradientPath(p_calc_);//梯度下降算法,路径更为光滑点
        ////方向滤波
        orientation_filter_ = new OrientationFilter();//进行方向滤波
//路径发布
        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
        //发布代价地图
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
//是否探索未知区域，flase--不可到达
        private_nh.param("allow_unknown", allow_unknown_, true);
        planner_->setHasUnknown(allow_unknown_);
        //窗口信息
        private_nh.param("planner_window_x", planner_window_x_, 0.0);
        private_nh.param("planner_window_y", planner_window_y_, 0.0);
        private_nh.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        private_nh.param("publish_scale", publish_scale_, 100);
        private_nh.param("outline_map", outline_map_, true);

        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

        dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb =
                [this](auto& config, auto level){ reconfigureCB(config, level); };
        dsrv_->setCallback(cb);

        initialized_ = true;
    } else
        ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

}

void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
    planner_->setLethalCost(config.lethal_cost);
    path_maker_->setLethalCost(config.lethal_cost);
    planner_->setNeutralCost(config.neutral_cost);
    planner_->setFactor(config.cost_factor);
    publish_potential_ = config.publish_potential;
    orientation_filter_->setMode(config.orientation_mode);
    orientation_filter_->setWindowSize(config.orientation_window_size);
}
    //清除成本图中的起始单元格，因为我们知道它不会成为障碍
void GlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }

    //set the associated costs in the cost map to be free
    costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);//自由空间
}

bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
    makePlan(req.start, req.goal, resp.plan.poses);

    resp.plan.header.stamp = ros::Time::now();
    resp.plan.header.frame_id = frame_id_;

    return true;
}

void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
    wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
    wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
}

/**
 *
 * @param wx
 * @param wy
 * @param mx
 * @param my
 这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。像素坐标
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
 */

bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
    double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
    double resolution = costmap_->getResolution();

    if (wx < origin_x || wy < origin_y)
        return false;

    /**
     * //参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
    //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

        if(!old_navfn_behavior_)
            convert_offset_ = 0.5;//false
        else
            convert_offset_ = 0.0;//true
    */
    mx = (wx - origin_x) / resolution - convert_offset_;
    my = (wy - origin_y) / resolution - convert_offset_;
   //  costmap_->getSizeInCellsX() 和 getSizeInCellsY：地图的大小，即像素数。
    if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
        return true;

    return false;
}


//todo 1.规划路径
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           std::vector<geometry_msgs::PoseStamped>& plan) {
    return makePlan(start, goal, default_tolerance_, plan);
}
//todo 2.规划路径
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
    boost::mutex::scoped_lock lock(mutex_);
    if (!initialized_) {
        //此计划器尚未初始化，但正在使用中，请在使用前调用initialize（）
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

    //clear the plan, just in case  清除计划，以防万一
    plan.clear();

    ros::NodeHandle n;
    std::string global_frame = frame_id_;
       //直到tf能够处理改变过去的事情。。。我们将要求目标在我们的全球框架内
    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    if (goal.header.frame_id != global_frame) {
        ROS_ERROR(
                "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
        return false;
    }

    if (start.header.frame_id != global_frame) {
        ROS_ERROR(
                "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
        return false;
    }

    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
    double start_x, start_y, goal_x, goal_y;

 /**
 worldToMap 该函数是在ROS Navigation Stack中的Costmap2D类中的一个方法，用于将给定的二维平面上的世界坐标 (wx, wy) 转换为该Costmap2D对象中相应的离散地图坐标 (start_x_i, start_y_i)。

这个函数返回一个布尔值，表示该世界坐标是否位于Costmap2D对象覆盖的区域内。如果该点在覆盖区域内，则返回true，否则返回false。

函数参数的意义如下：

    wx：待转换点的x坐标，以世界坐标系为准。
    wy：待转换点的y坐标，以世界坐标系为准。
    start_x_i：返回的转换后的x坐标，以离散地图坐标系为准。
    start_y_i：返回的转换后的y坐标，以离散地图坐标系为准。

函数的主要作用是将给定的世界坐标转换为Costmap2D对象中对应的离散地图坐标，以便进行后续的路径规划等操作。如果该点不在地图覆盖区域内，则表示该点无法被使用，需要进行相应的处理（例如，忽略该点或重新规划路径）。

  bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
  if (wx < origin_x_ || wy < origin_y_) {
    return false;
  }

  mx = static_cast<int>((wx - origin_x_) / resolution_);
  my = static_cast<int>((wy - origin_y_) / resolution_);

  if (mx < size_x_ && my < size_y_) {
    return true;
  }

  return false;
}
以下是一个简单的示例实现，用于演示 worldToMap 函数的实现过程：

c++

bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
  if (wx < origin_x_ || wy < origin_y_) {
    return false;
  }

  mx = static_cast<int>((wx - origin_x_) / resolution_);
  my = static_cast<int>((wy - origin_y_) / resolution_);

  if (mx < size_x_ && my < size_y_) {
    return true;
  }

  return false;
}

这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
离散地图坐标是什么意思与世界坐标的关系是什么？为什么要转成离散地图坐标？
离散地图坐标是指地图中离散化的像素坐标，通常用于表示地图上的栅格，而世界坐标则是指地图所在的实际坐标系中的坐标，通常用于表示地图上的物理位置。

在机器人的导航与运动控制中，通常需要将世界坐标系中的目标位置转换为机器人所处的局部坐标系中的相对位置，从而使机器人能够更好地实现目标位置的控制。这时，离散地图坐标就显得尤为重要，因为它可以将目标位置在地图中进行离散化，从而更容易进行处理。

具体来说，离散地图坐标通常用于表示机器人在地图上的位置和方向，同时也用于描述地图上的障碍物和其它特征。将世界坐标转换为离散地图坐标，则可以方便地进行地图上的路径规划、避障等操作。

在实际应用中，转换为离散地图坐标也有助于减小数据量和加速处理速度。例如，使用离散地图坐标来描述障碍物信息，可以避免对地图中每个点进行处理，而只需要处理离散的栅格信息，从而提高算法的运行效率。
  */
    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {//世界坐标转像素坐标
    //机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗？
        ROS_WARN(
                "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }

        //参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
        //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

        if(old_navfn_behavior_){
        start_x = start_x_i;
        start_y = start_y_i;
    }else{
        worldToMap(wx, wy, start_x, start_y);//世界坐标转像素坐标
    }

    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
        ROS_WARN_THROTTLE(1.0,
                "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
        return false;
    }
    if(old_navfn_behavior_){
        goal_x = goal_x_i;
        goal_y = goal_y_i;
    }else{
        worldToMap(wx, wy, goal_x, goal_y);
    }

    //clear the starting cell within the costmap because we know it can't be an obstacle 清除成本图中的起始单元格，因为我们知道它不会成为障碍
    clearRobotCell(start, start_x_i, start_y_i);
    //nx 和 ny 分别表示数组 地图 的宽度和高度
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();

    //make sure to resize the underlying array that Navfn uses 确保调整Navfn使用的基础数组的大小
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny];

     /**
     具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
     然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。
    在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
     于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
      */

        if(outline_map_)
            outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。


            ///home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/astar.cpp 53
            ///home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/dijkstra.cpp 80
            //planner_- 用a* 还是 dijkstra 做全局规划 175
            /**
             * //potential（预估每个点到目标节点的距离）
             * 这段代码是在进行路径规划，其中调用了planner_的calculatePotentials函数，该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组。其中，costmap_->getCharMap()是获取地图的信息，
             * start_x, start_y, goal_x, goal_y是起点和终点的坐标，nx * ny * 2是数组potential_array_的大小。
             * 函数返回值是一个bool类型的变量found_legal，表示是否找到了一条合法的路径。
             *
             * 在这段代码中，costmap_->getCharMap()返回的是一个unsigned char类型的指针，指向的是一个二维数组，数组中的每个元素表示对应位置的cell的状态。在这个数组中，0表示自由空间，255表示障碍物，其他值表示不同的占用程度。
             * 对不起，我的之前的回答有误。在ROS中，costmap_->getCharMap() 返回的数据类型是指向 unsigned char 类型的一维数组的指针。这个一维数组的长度等于地图的宽度乘以高度。每个数组元素对应着地图中一个单元格，其值表示该单元格的状态。
             * 通常，0 表示自由空间，100 表示障碍物，而其他的值则可以用于表示其他状态。
             *
             * nx * ny * 2
            这个参数是用来指定potential_array_数组的大小的，这个数组是用来存储每个点到目标点的距离的。在这个函数中，potential_array_数组的大小是nx * ny * 2，
             其中nx和ny是地图的宽度和高度，2是因为在计算距离时，需要同时计算正向和反向的距离，因此数组的大小需要乘以2。在这个函数中，nx和ny的值是通过costmap_的getSizeInCellsX()和getSizeInCellsY()函数获取的，
             这两个函数返回的是地图的宽度和高度，单位是cell。因此，nx * ny * 2实际上是地图中所有cell的数量乘以2。
             */
            bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                        nx * ny * 2, potential_array_);//float* potential_array_; 该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组

        if(!old_navfn_behavior_)
            planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);

       /**
        * 这个函数与getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)函数的区别在于，getPlanFromPotential函数是用于从潜在代价地图中获取路径的函数，
        * 而publishPotential函数是用于发布潜在代价地图的函数。getPlanFromPotential函数的输入是起点和终点的坐标，
        * 以及一个空的路径plan，函数的输出是一个bool类型的值，表示是否成功获取到路径。如果成功获取到路径，则路径会被存储在plan中。publishPotential函数的输入是一个浮点型数组，
        * 表示每个点到目标节点的距离，函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。函数首先获取代价地图的大小和分辨率，然后将整个代价地图发布出去。
        * 在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
        */

        if(publish_potential_)
            publishPotential(potential_array_);

        if (found_legal) {
            //extract the plan
            /**
             * 这个函数是用来从potential_array_数组中获取一条从起点到终点的路径的。在这个函数中，start_x和start_y是起点的坐标，goal_x和goal_y是终点的坐标，
             * goal是一个geometry_msgs::PoseStamped类型的变量，表示终点的位置，plan是一个std::vector<geometry_msgs::PoseStamped>类型的变量，用来存储路径上的所有点。在函数中，
             * 首先会调用calculatePotentials()函数计算每个点到终点的距离，然后再从potential_array_ （代价）数组中获取一条从起点到终点的路径，并将路径上的所有点存储到plan变量中。


             */
            if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
                //make sure the goal we push on has the same timestamp as the rest of the plan 确保我们推进的目标与计划的其余部分具有相同的时间戳
                geometry_msgs::PoseStamped goal_copy = goal;
                goal_copy.header.stamp = ros::Time::now();
                plan.push_back(goal_copy);
            } else {
                ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");//发现合法潜在客户时，未能从潜在客户处获取计划。这不应该发生
            }
        }else{
            ROS_ERROR_THROTTLE(5.0, "Failed to get a plan.");//无法获取计划
        }

        // add orientations if needed 如果需要，添加方向
        orientation_filter_->processPath(start, plan);

        //publish the plan for visualization purposes 出于可视化目的发布计划
        publishPlan(plan);
        delete[] potential_array_;
        return !plan.empty();
    }
    //出于可视化目的发布计划
    void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return;
        }

        //create a message for the plan
        nav_msgs::Path gui_path;
        gui_path.poses.resize(path.size());

        gui_path.header.frame_id = frame_id_;
        gui_path.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < path.size(); i++) {
            gui_path.poses[i] = path[i];
        }

        plan_pub_.publish(gui_path);
    }

    bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
                                          const geometry_msgs::PoseStamped& goal,
                                           std::vector<geometry_msgs::PoseStamped>& plan) {
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }

        std::string global_frame = frame_id_;

        //clear the plan, just in case
        plan.clear();

        std::vector<std::pair<float, float> > path;
             // float* potential_array_;   //potential（预估每个点到目标节点的距离）
             //191  path_maker_ = new GridPath(p_calc_);//new 出path_maker_实例，从可行点中提取路径  ||  path_maker_ = new GradientPath(p_calc_);//梯度下降算法,路径更为光滑点
        if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {//start_x, start_y, goal_x, goal_y 像素坐标
            ROS_ERROR("NO PATH!");
            return false;
        }

        ros::Time plan_time = ros::Time::now();
        for (int i = path.size() -1; i>=0; i--) {
            std::pair<float, float> point = path[i];
            //convert the plan to world coordinates
            double world_x, world_y;
            mapToWorld(point.first, point.second, world_x, world_y);

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = plan_time;
            pose.header.frame_id = global_frame;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            plan.push_back(pose);
        }
        if(old_navfn_behavior_){
                plan.push_back(goal);
        }
        return !plan.empty();
    }
//potential（预估每个点到目标节点的距离） 发布潜力    potential_array_ 潜力数组
/**
 * 这个函数是一个全局路径规划器的类中的一个函数，用于发布潜在代价地图。函数的输入是一个浮点型数组，表示每个点到目标节点的距离。函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。
 * 函数首先获取代价地图的大小和分辨率，
 * 然后将整个代价地图发布出去。在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
 * @param potential
 */
    void GlobalPlanner::publishPotential(float* potential)
    {
        int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
        double resolution = costmap_->getResolution();
        nav_msgs::OccupancyGrid grid;
        // Publish Whole Grid
        grid.header.frame_id = frame_id_;
        grid.header.stamp = ros::Time::now();
        grid.info.resolution = resolution;

        grid.info.width = nx;
        grid.info.height = ny;

        double wx, wy;
        costmap_->mapToWorld(0, 0, wx, wy);
        grid.info.origin.position.x = wx - resolution / 2;
        grid.info.origin.position.y = wy - resolution / 2;
        grid.info.origin.position.z = 0.0;
        grid.info.origin.orientation.w = 1.0;

        grid.data.resize(nx * ny);

        float max = 0.0;
        for (unsigned int i = 0; i < grid.data.size(); i++) {
            float potential = potential_array_[i];
            if (potential < POT_HIGH) {//potential（预估每个点到目标节点的距离） 值
                if (potential > max) {
                    max = potential;
                }
            }
        }

        for (unsigned int i = 0; i < grid.data.size(); i++) {
            if (potential_array_[i] >= POT_HIGH) {
                grid.data[i] = -1;
            } else {
                if (fabs(max) < DBL_EPSILON) {
                    grid.data[i] = -1;
                } else {
                    grid.data[i] = potential_array_[i] * publish_scale_ / max;
                }
            }
        }
        potential_pub_.publish(grid);
    }

    } //end namespace global_planner

